/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Test;
import Behaviors.Schemas.Perception.NxPSMapVP;
import Utils.VectorFrame;
import Behaviors.Arbitrators.*;
import Behaviors.Schemas.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;

import Robot.*;
import Robot.Sensors.*;
import javaclient3.structures.PlayerConstants;
import Utils.*;
import javaclient3.*;
import javax.swing.*;

/**
 *
 * @author alejo
 */
public class pruebas2 {
public static void main(String args[])
    {

        boolean isSimulated = false;
        boolean isUsingVp = false;
        boolean showArbFrame = true;
        boolean showMapObstFrame = false;


        Robot real;
        Sensor ir;
        //Creacion de la instancia robot.
        if(isSimulated)
            real = new Robot("real","localhost",6665, 0);
        else
            real = new Robot("real","192.168.0.1",6665, 0);

        RealPosition2d posReal;


        try{

            posReal = new RealPosition2d(real,"realPos",0);

        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }






        try{
            if(isSimulated)
                ir = new Sonar(real,"realIR",0);
            else
                ir = new IR(real,"realIR",0);


        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }


        //VAnish point sensor
        VPSensor vp = new VPSensor(640,480,"localhost",25557);
        if(isUsingVp)
            vp.start();

        real.runThreaded(5, -1);

        if(isSimulated)
            while(!((Sonar)ir).updatePoses());
        else
            while(!((IR)ir).updatePoses());


        posReal.setMotorPower(true);


        while(true)
        {

            float[] result = null;
            while(result == null)
                result = ((IR)ir).read(Sensor.READ_MODE_WAIT);



            int resultadoIR = 0;
            for(int i=0; i<result.length;i++)
            {
                if(result[i] == 0)
                {
                    switch(i)
                    {
                        case 0:
                            resultadoIR += 1;
                            break;
                        case 1:
                            resultadoIR += 2;
                            break;
                        case 2:
                            resultadoIR += 4;
                            break;
                        case 3:
                            resultadoIR += 8;
                            break;
                        case 4:
                            resultadoIR += 16;
                            break;
                        case 5:
                            resultadoIR += 32;
                            break;
                    }
                }


            }

            System.out.println(resultadoIR);


            switch(resultadoIR)
            {
                case 1:
                case 2:
                case 3:
                case 6:
                case 7:
                case 15:
                case 31:
                    posReal.setSpeed(0, -0.5f);
                    break;

                case 4:
                case 8:
                case 12:
                case 30:

                    posReal.setSpeed(-0.5f,0.5f);
                    break;
                case 16:
                case 32:
                case 48:
                case 24:
                case 56:
                case 28:
                case 60:
                case 62:
                    posReal.setSpeed(0, 0.5f);
                    break;
                default:
                    posReal.setSpeed(0.8f, 0);
            }
        }

    }
}
